/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2025-05-13     Administrator       the first version
 */
#include "balance.h"

//x为前后，+为前 | y为左右，+为右 | z为顺逆，+为顺，(Axle_spacing+Wheel_spacing)已经是一半了
//A左上B左下C右下D右上

//统一设定PWM为正值时，轮子是向前转，因此需要调整pin脚规定好正负方向
//由于编码器电机是对称安装的，所以需要将其中一侧的两个轮子的前进方向的编码器值规定为正方向，最好是正值
//位置环和速度环并入速度环做计算时，一样要考虑正负号

//速度环，位置环，角度环共用一组PID控制式
//需要分别定义它们三个环的不同pid结构体变量
//目前只用一个速度环控制
//若要使用位置环控制，需要来一个软件定时器定时在20ms内获取编码器的计数值并更新我，来个线程就好


/*主任务运行状态机定义*/
//typedef enum
//{
//
//} balance_work_t;

float PWM_A, PWM_B, PWM_C, PWM_D;

/*输出四个电机的PWM*/
/*A左前，B左后，C右后，D右前*/
void Drive_motor(rt_int32_t PWMA,rt_int32_t PWMB,rt_int32_t PWMC,rt_int32_t PWMD)
{

  if(PWMA>=0)//左前
  {
      /*设置引脚正负*/
      rt_pin_write(Left_ahead_AIN1, PIN_HIGH);
      rt_pin_write(Left_ahead_AIN2, PIN_LOW);

      /* 设置PWM周期和脉冲宽度默认值 */
      rt_pwm_set(pwm_dev12, 1, PERIOD, (rt_uint32_t)PWMA);  /*单位为纳秒ns */
      /* 使能设备 */
      rt_pwm_enable(pwm_dev12, 1);
  }
  else
  {
      /*设置引脚正负*/
      rt_pin_write(Left_ahead_AIN1, PIN_LOW);
      rt_pin_write(Left_ahead_AIN2, PIN_HIGH);

      /* 设置PWM周期和脉冲宽度默认值 */
      rt_pwm_set(pwm_dev12, 1, PERIOD, -(rt_uint32_t)PWMA);
      /* 使能设备 */
      rt_pwm_enable(pwm_dev12, 1);
  }

  if(PWMB>=0)//左后
  {
      /*设置引脚正负*/
      rt_pin_write(Left_back_BIN1, PIN_HIGH);
      rt_pin_write(Left_back_BIN2, PIN_LOW);

      /* 设置PWM周期和脉冲宽度默认值 */
      rt_pwm_set(pwm_dev12, 2, PERIOD, (rt_uint32_t)PWMB);
      /* 使能设备 */
      rt_pwm_enable(pwm_dev12, 2);
//      rt_kprintf("Driver ok\r\n");
  }
  else
  {
      /*设置引脚正负*/
      rt_pin_write(Left_back_BIN1, PIN_LOW);
      rt_pin_write(Left_back_BIN2, PIN_HIGH);

      /* 设置PWM周期和脉冲宽度默认值 */
      rt_pwm_set(pwm_dev12, 2, PERIOD, -(rt_uint32_t)PWMB);
      /* 使能设备 */
      rt_pwm_enable(pwm_dev12, 2);
  }

  if(PWMC>=0)//右后
  {
      /*设置引脚正负*/
      rt_pin_write(right_back_BIN1, PIN_HIGH);
      rt_pin_write(right_back_BIN2, PIN_LOW);

      /* 设置PWM周期和脉冲宽度默认值 */
      rt_pwm_set(pwm_dev5, 4, PERIOD, (rt_uint32_t)PWMC);
      /* 使能设备 */
      rt_pwm_enable(pwm_dev5, 4);
//      rt_kprintf("Driver ok\r\n");
  }
  else
  {
      /*设置引脚正负*/
      rt_pin_write(right_back_BIN1, PIN_LOW);
      rt_pin_write(right_back_BIN2, PIN_HIGH);

      /* 设置PWM周期和脉冲宽度默认值 */
      rt_pwm_set(pwm_dev5, 4, PERIOD, -(rt_uint32_t)PWMC);
      /* 使能设备 */
      rt_pwm_enable(pwm_dev5, 4);
  }

  if(PWMD>=0)//右前
  {
      /*设置引脚正负*/
      rt_pin_write(right_ahead_AIN1, PIN_HIGH);
      rt_pin_write(right_ahead_AIN2, PIN_LOW);

      /* 设置PWM周期和脉冲宽度默认值 */
      rt_pwm_set(pwm_dev5, 3, PERIOD, (rt_uint32_t)PWMD);
      /* 使能设备 */
      rt_pwm_enable(pwm_dev5, 3);
//      rt_kprintf("Driver ok\r\n");
  }
  else
  {
      /*设置引脚正负*/
      rt_pin_write(right_ahead_AIN1, PIN_LOW);
      rt_pin_write(right_ahead_AIN2, PIN_HIGH);

      /* 设置PWM周期和脉冲宽度默认值 */
      rt_pwm_set(pwm_dev5, 3, PERIOD, -(rt_uint32_t)PWMD);//初始化先不动
      /* 使能设备 */
      rt_pwm_enable(pwm_dev5, 3);
  }

}


void MOVE(float angle,float speed, float w_z)//speed在这里的单位是圈/秒
{
    float Target_Angle=(float)angle/360*2*PI;//角度转弧度

    float Target_Speed_X=speed*cos(Target_Angle);//x方向目标速度
    float Target_Speed_Y=speed*sin(Target_Angle);//y方向目标速度
    float Target_Speed_W = w_z;                         // Z方向角速度（自转）

    Move_Transform(Target_Speed_X,Target_Speed_Y,Target_Speed_W);
}


/*主控制函数*/
void MAIN_Control(float Vx,float Vy,float Vz)
{
    float Car_HW=Robot_Parament.WheelSpacing + Robot_Parament.AxleSpacing; // 已经是一半了
    /*电机速度目标值*/
    PWM_A = Vx+Vy+Vz*Car_HW;
    PWM_B = Vx-Vy+Vz*Car_HW;
    PWM_C = Vx+Vy-Vz*Car_HW;
    PWM_D = Vx-Vy-Vz*Car_HW;
    /*pid计算和电机PWM赋值在app_pid.c中*/

}


/*主要使用在Move_Transform和Control_Move里面*/
/*控制小车做任务的函数*/
void Balance_task(void *pvParameters)
{
    char circle = 1;
    char label_temp[20] = {0};//标签临时值
    static rt_uint8_t conut = 0;//玩具计数值
    static uint8_t State_B = Find_Situ;//初始状态
    JQ8x00_Command_Data(AppointTrack, 5);//开始抓取玩具

    while(1)
    {
        switch (State_B) {
            case Find_Situ:
                //原地不动查看摄像头画面有玩具，有就转到追玩具状态
                if (strcmp(toy_label, "ball") == 0 ||strcmp(toy_label, "gonk") == 0 ||strcmp(toy_label, "figure") == 0)
                {
                    Drive_motor(0, 0, 0, 0);
                    set_base_speed(0);
                    State_B = Trace;
                }
                //原地不动查看摄像头画面无玩具，原地旋转直到找到玩具
                else {
                    set_base_speed(20);
//                    Drive_motor(10000, 10000, 10000, 10000);
                }
                break;
            case Trace:
                active_K230_position_pid();//打开追玩具的pid
                State_B = Judge_one;
                break;
            case Judge_one:
                if((toy_x <= 415 && toy_x >= 385) && (toy_distance <=28 && toy_distance >=20))
                {
                    Drive_motor(0, 0, 0, 0);
                    deactive_K230_position_pid();//关掉追的pid
                    JXB_send_byte(1, 1);//机械臂运动到准备抓的位置
//                    active_angle_pid();//打开角度环pid走直线
                    set_base_speed(15);//向前走直到找到玩具
                    State_B = Judge_two;
                    //机械臂运动到准备抓的位置需要50ms
                    rt_thread_mdelay(1500);
                }
                break;
            case Judge_two:
                if (strcmp(toy_label, "ball") == 0 ||strcmp(toy_label, "gonk") == 0 ||strcmp(toy_label, "figure") == 0)
                {
                    set_base_speed(0);
                    Drive_motor(0, 0, 0, 0);
//                    deactive_angle_pid();//关掉角度环
                    active_K230_catch_pid();//打开机械臂抓的pid
                    State_B = Catch;
                }
                break;
            case Catch:
                if((toy_x <= 405 && toy_x >= 395) && (toy_y <=246 && toy_y >=233))
                {
                    strncpy(label_temp, toy_label, sizeof(label_temp) - 1);
                    label_temp[sizeof(label_temp) - 1] = '\0';  // 确保结尾是 \0
                    deactive_K230_catch_pid();//关掉机械臂抓的pid
                    JXB_send_byte(7, 1);//抓,如蜡笔小新
                    //抓玩具这个动作需要大概800ms
                    Drive_motor(0, 0, 0, 0);
                    State_B = Put_toy;
                    rt_thread_mdelay(2000);  // 800ms
                }
                break;
            case Put_toy:
                if(strcmp(label_temp, "ball") == 0)
                {
                    JQ8x00_Command_Data(AppointTrack, 1);//已拾取小球
                    //准备放球
                    JXB_send_byte(5, 1);
                    State_B = Put_finish;
                    //准备放球这个动作大概需要1500ms
                    rt_thread_mdelay(2000);
                }
                else if(strcmp(label_temp, "gonk") == 0)
                {
                    JQ8x00_Command_Data(AppointTrack, 2);//已拾取毛绒玩具
                    //准备放gonk
                    JXB_send_byte(3, 1);
                    State_B = Put_finish;
                    //准备放gonk这个动作大概需要1000ms
                    rt_thread_mdelay(2000);
                }
                else if(strcmp(label_temp, "figure") == 0)
                {
                    JQ8x00_Command_Data(AppointTrack, 3);//已拾取蜡笔小新
                    //准备放figure
                    JXB_send_byte(8, 1);
                    State_B = Put_finish;
                    //准备放figure这个动作大概需要1000ms，for循环来一个大概的延时
                    rt_thread_mdelay(2000);
                }
                break;
            case Put_finish:
                if(strcmp(label_temp, "ball") == 0)
                {
                    conut ++;
                    //放下球
                    JXB_send_byte(6, 1);
                    State_B = PanDuan;
                    //放下球这个动作大概需要50ms
                    rt_thread_mdelay(500);
                }
                else if(strcmp(label_temp, "gonk") == 0)
                {
                    conut ++;
                    //放下gonk
                    JXB_send_byte(4, 1);
                    State_B = PanDuan;
                    //放下gonk这个动作大概需要100ms，for循环来一个大概的延时
                    rt_thread_mdelay(500);
                }
                else if(strcmp(label_temp, "figure") == 0)
                {
                    conut ++;
                    //放下figure
                    JXB_send_byte(9, 1);
                    State_B = PanDuan;
                    //放下figure这个动作大概需要50ms，for循环来一个大概的延时
                    rt_thread_mdelay(500);
                }
            case PanDuan:
                //判断抓取的玩具数量是否到达了4个
                if(conut == 7)
                {
                    //机械臂运动到找家的位置
                    JQ8x00_Command_Data(AppointTrack, 4);//已全部拾取完毕
                    JXB_send_byte(15, 1);
                    //回到这个动作大概需要1000ms，for循环来一个大概的延时
                    State_B = Disable;//到达了四个，状态转至找家
                    rt_thread_mdelay(1000);
//                    rt_thread_mdelay();
                }
                else {
                    //未到达了四个，回到2准备抓查看视角内有无其它玩具
                    JXB_send_byte(14, 1);
                    State_B = Find_Situ;
                    //回到这个动作大概需要1500ms，for循环来一个大概的延时
                    rt_thread_mdelay(1700);
                }
                break;
            case Trace_home:
                //找到了家
                if (strcmp(toy_label, "home") == 0)
                {
                    //停止，并转到Judge_one_home状态
                    Drive_motor(0, 0, 0, 0);
                    Circle_Move(0);
                    active_K230_position_pid();//打开对准家的环
                    State_B = Judge_one_home;
                }
                else {
                    //没找到家，原地转圈直到找到家
                    Circle_Move(15);
                }
                break;
            case Judge_one_home:
                //判断是否到预设位置
                if((toy_x <= 410 && toy_x >= 390) && (toy_distance <=35 && toy_distance >=25))
                {
                    Drive_motor(0, 0, 0, 0);
                    deactive_K230_position_pid();//关掉追的pid
                    JXB_send_byte(1, 1);//机械臂运动到准备抓的状态
                    active_angle_pid();//打开角度环pid走直线
                    set_base_speed(12);//向前走直到找到家
                    //运动到这个状态大概需要50ms
                    rt_thread_mdelay(50);
                    State_B = Judge_two_home;
                }
                break;
            case Judge_two_home:
                //视角内出现家
                if (strcmp(toy_label, "home") == 0)
                {
                    Drive_motor(0, 0, 0, 0);
                    set_base_speed(0);//车停止
                    deactive_angle_pid();//关掉角度环
                    active_K230_catch_pid();//打开机械臂抓的pid
                    State_B = Catch_home;
                }
                break;
            case Catch_home:
                //判断家是否到达预设位置
                if((toy_x <= 405 && toy_x >= 395) && (toy_y <=246 && toy_y >=233))
                {
                    deactive_K230_catch_pid();//关掉机械臂抓的pid
                    Drive_motor(0, 0, 0, 0);
                    //回收机械臂
                    JXB_send_byte(12, 1);
                    State_B = Back_home;
                    rt_thread_mdelay(1500);
                }
                break;
            case Back_home:
                set_base_speed(13);
                //for循环等待一会后停止，回到home
                rt_thread_mdelay(100);
                break;
            case Arrive_home:
                Drive_motor(0, 0, 0, 0);
                set_base_speed(0);
                State_B = Disable;
            case Disable:
                break;
            default:
                break;
        }
        rt_thread_mdelay(10);
    }
}
